function [scheduledTargets, scheduledPriorities, status] = GRASP_AddToSolution(targetId, priority, scheduledTargets, scheduledPriorities, olTargets, olBins)
%GRASP_AddToSolution Summary of this function goes here
%   Detailed explanation goes here

if length(scheduledTargets) < 1
  scheduledTargets = targetId;
  scheduledPriorities = priority;
  status = 1;
  return;
end
  
tempScheduledTargets = scheduledTargets;
tempScheduledPriorities = scheduledPriorities;

% bin = 0;
% for k = 1:length(olTargets)
%   if targetId == olTargets(k)
%     bin = olBins(k);
%     break;
%   end
% end
bin = olBins(targetId == olTargets);

% bins(1:length(scheduledTargets)) = 0;
% for n = 1:length(scheduledTargets)
%   for k = 1:length(olTargets)
%     if scheduledTargets(n) == olTargets(k)
%       bins(n) = olBins(k);
%       break;
%     end
%   end
% end
bins(1:length(scheduledTargets)) = 0;
for k = 1:length(scheduledTargets)
  bins(k) = olBins(scheduledTargets(k) == olTargets);
end

% insertPoint = length(bins);
% for n = 1:length(bins)
%   if bin <= bins(n)  
%     insertPoint = n - 1;
%     break;
%   end
% end
insertPoints = find(bins == bin);

%scheduledTargets = [tempScheduledTargets(1:insertPoint) targetId tempScheduledTargets(insertPoint+1:length(tempScheduledTargets))];
%scheduledPriorities = [tempScheduledPriorities(1:insertPoint) priority tempScheduledPriorities(insertPoint+1:length(tempScheduledPriorities))];

% for n = insertPoint+1:length(bins)
%   if bin == bins(n)
%     % another potential solution
%     scheduledTargets = [scheduledTargets; tempScheduledTargets(1:n) targetId tempScheduledTargets(n+1:length(tempScheduledTargets))]; %#ok<AGROW>
%     scheduledPriorities = [scheduledPriorities; tempScheduledPriorities(1:n) priority tempScheduledPriorities(n+1:length(tempScheduledPriorities))]; %#ok<AGROW>
%   else
%     break;
%   end
% end

if length(insertPoints) < 1
  insertPoint = length(bins);
  for n = 1:length(bins)
    if bin <= bins(n)  
      insertPoint = n - 1;
      break;
    end
  end
  
  scheduledTargets = [tempScheduledTargets(1:insertPoint) targetId tempScheduledTargets(insertPoint+1:length(tempScheduledTargets))];
  scheduledPriorities = [tempScheduledPriorities(1:insertPoint) priority tempScheduledPriorities(insertPoint+1:length(tempScheduledPriorities))];
else
  scheduledTargets = [];
  scheduledPriorities = [];

  insertPoints = [insertPoints(1)-1 insertPoints];
  
  for k = 1:length(insertPoints)
    n = insertPoints(k);
    scheduledTargets = [scheduledTargets; tempScheduledTargets(1:n) targetId tempScheduledTargets(n+1:length(tempScheduledTargets))]; %#ok<AGROW>
    scheduledPriorities = [scheduledPriorities; tempScheduledPriorities(1:n) priority tempScheduledPriorities(n+1:length(tempScheduledPriorities))]; %#ok<AGROW>
  end
end

status = 1;
end